package fch;

import robocode.Robot;

/**
 * A Robot object with the ability to move to a specified point.
 * @author Christopher Foo
 *
 */
public abstract class MovingRobot extends Robot {
  
  /**
   * Moves the robot to the specified point.
   * @param x The x-coordinate of the point to move to.
   * @param y The y-coordinate of the point to move to.
   */
  public void moveToPoint(double x, double y) {
    double actualX = x; // Actual X coordinate to move to.
    double actualY = y; // Actual Y coordinate to move to.
    while (Math.round(getX()) != Math.round(actualX) || Math.round(getY()) != Math.round(actualY)) {
      
      //Account for the robot's width to prevent attempts to send the robot to an unreachable point.
      if (actualX < getWidth() / 2) {
        actualX = getWidth() / 2;
      }
      else if (actualX > getBattleFieldWidth() - getWidth() / 2) {
        actualX = getBattleFieldWidth() - getWidth() / 2;
      }
      
      //Account for the robot's height to prevent attempts to sent the robot to an unreachable 
      //point.
      if (actualY < getHeight() / 2) {
        actualY = getHeight() / 2;
      }
      else if (actualY > getBattleFieldHeight() - getHeight() / 2) {
        actualY = getBattleFieldHeight() - getHeight() / 2;
      }
      
      moveToPointAttempt(actualX, actualY);
    }
  }
  
  /**
   * Attempts to move the robot to the specified point.
   * @param x The x-coordinate of the point to move to.
   * @param y The y-coordinate of the point to move to.
   */
  public void moveToPointAttempt (double x, double y) {
    double currentHeading = getHeading(); //The robot's current heading.
    double desiredHeading; //The heading to the point to move to.
    double headingDifference; //The difference between the current and the desired headings.
    double currentX = getX(); //The robot's current x position.
    double currentY = getY(); //The robot's current y position.
    
    //Comparison of the robot's current x position to the desired x position.
    //>0 if current is larger
    // 0 if equal
    //<0 if smaller
    int compareX = ((Double) currentX).compareTo(x); 
    
    //Comparison of the robot's current y position
    //to the desired y position.
    //>0 if current is larger
    // 0 if equal
    //<0 if smaller
    int compareY = ((Double) currentY).compareTo(y); 

    if (compareY == 0) {
      
      //On Point
      if (compareX == 0) {
        return;
      }
      
      //East of Point
      else if (compareX > 0) {
        desiredHeading = 270;
      }
      
      //West of Point
      else {
        desiredHeading = 90;
      }
    }
    else if (compareY > 0) {
      
      //Directly North of Point
      if (compareX == 0) {
        desiredHeading = 180;
      }
      
      //North-East of Point
      else if (compareX > 0) {
        desiredHeading = Math.toDegrees(Math.abs(Math.atan((currentX - x) / (currentY - y)))) 
                         + 180;
      }
      
      //North-West of Point
      else {
        desiredHeading = 180 
                         - Math.toDegrees(Math.abs(Math.atan((x - currentX) / (currentY - y))));

      }
    }
    
    //Else compareY must be < 0
    else {
      
      //Directly South of Point
      if (compareX == 0) {
        desiredHeading = 180;
      }
      
      //South-East of Point
      else if (compareX > 0) {
        desiredHeading = Math.toDegrees(Math.abs(Math.atan((y - currentY) / (currentX - x)))) 
                         + 270;
      }
      
      //South-West of Point
      else {
        desiredHeading = 90 
                         - Math.toDegrees(Math.abs(Math.atan((y - currentY) / (x - currentX))));
      }
    }
    
    //Negative if desiredHeading is more clockwise 
    //Positive if desiredHeading is more counter-clockwise
    //Absolute Value = difference in degree angles
    headingDifference = currentHeading - desiredHeading;
    
    //Would be large counter-clockwise turn so make smaller clockwise turn
    if (headingDifference > 180) {
      turnRight(360 - headingDifference);
    }
    
    //Would be large clockwise turn so make smaller counter-clockwise turn
    else if (headingDifference < -180) {    
      //headingDifference is negative so add instead of subtract to get turning angle
      turnLeft(360 + headingDifference);
    }
    
    else {
      turnLeft(headingDifference);
    }
    
    ahead(Math.hypot(currentX - x, currentY - y));
  }
}
